/****************************************Copyright (c)*******************************************************
**                               河南天一航天科技有限责任公司
**                                     
**                                 http://www.???.com
**
**--------------File Info-------------------------------------------------------------------------------
** 文 件 名 : 数据采集任务
** 作    者 : 
** 版    本 ：V1.0
** 完成时间 : 2023年7月16日
** 描    述 ：在20ms内: 分2步采集电机电流[mA]共12个, 扫描16路编码器角度值[度],并求出角速度[度/s]
**------------------------------------------------------------------------------------------------------
** 修 改 人 ：
** 版    本 ：
** 修改时间 : 年月日
** 修改内容 : 
************************************************************************************************************/

/*
IIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIII
*											 头文件引用区		
#include ...
IIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIII
*/
#include "Includes.h"
#include "eeprom.h"

#pragma region //===字段===
#define SAMPLE_T	40	//编码器数据采样周期ms
#define CODER_NUM	4	//编码器个数
#define CODER_MAX_VAR				16384


static GPIO_TypeDef *In_Ports[] = { 
	IN1_GPIO_Port, IN2_GPIO_Port, IN3_GPIO_Port, IN4_GPIO_Port, IN5_GPIO_Port,
	IN6_GPIO_Port, IN7_GPIO_Port, IN8_GPIO_Port, IN9_GPIO_Port, IN10_GPIO_Port
};
static uint16_t In_Pins[] = { 
	IN1_Pin, IN2_Pin, IN3_Pin, IN4_Pin, IN5_Pin,
	IN6_Pin, IN7_Pin, IN8_Pin, IN9_Pin, IN10_Pin};
#pragma endregion

#pragma region //===变量===
extern osSemaphoreId SPI3_CpltSemHandle;
extern osThreadId defaultTaskHandle;
SysState_t SysState; //系统工作状态
int Angle[CODER_NUM];	//编码器角度采集值:0~16384
uint16_t Analogs[CODER_NUM];	//规格化0~1000后的模拟量
uint8_t Keys[4];	//开关按键状态
#pragma endregion

#pragma region //===============重写方法=================
extern uint8_t mac[];
extern uint8_t subnet[];
extern uint8_t gateway[];
int LoadEEPROM_Var(void)
{
	//装载默认参数
	memcpy(EEPROM_MSG.mac, mac, 6);
	memcpy(EEPROM_MSG.lip, local_ip, 4);
	memcpy(EEPROM_MSG.sub, subnet, 4);
	memcpy(EEPROM_MSG.gw, gateway, 4);
	memcpy(EEPROM_MSG.remote_ip, remote_ip, 4);
	EEPROM_MSG.local_port = local_port;
	EEPROM_MSG.remote_port = remote_port;
	// 初始化应用程序变量
	memset(EEPROM_MSG.analog_pos, -1, 4);
	memset(EEPROM_MSG.key_pos, -1, 10);
	memset(EEPROM_MSG.out_pos, -1, 15);
	EEPROM_MSG.device_id = 0xFF;
	memset(EEPROM_MSG.analog_tap_pos, -1, 40 * 2);
	memset(EEPROM_MSG.freq_pwm_max, -1, 15 * 2);
	memset(EEPROM_MSG.cal_pct, -1, 30 * 2);
	memset(EEPROM_MSG.cal_out, -1, 30 * 2);
	EEPROM_MSG.cal_pct[0] = 0;
	EEPROM_MSG.cal_pct[1] = 1000;
	EEPROM_MSG.cal_pct[10] = 0;
	EEPROM_MSG.cal_pct[11] = 1000;
	EEPROM_MSG.cal_pct[20] = 0;
	EEPROM_MSG.cal_pct[21] = 1000;

	int ret = EE_Init();
	if (ret == EE_OK)
	{
		ret = EE_LoadAllVar((uint16_t*)&EEPROM_MSG); //从eeprom中装载数据
		if (ret != EE_OK)
		{
			EE_SaveAllVar((uint16_t*)&EEPROM_MSG); /*使用全部或部分的默认信息，存储到EEPROM中*/
		}
	}
	osThreadResume(defaultTaskHandle);
	return ret;
}
#pragma endregion

#pragma region //==============私有函数==============
/**
*@brief	  微秒延时函数
*@param		time_ms:要延时微秒时间数
*@return	无
*/
void delay_us(uint32_t time_us)
{
	time_us *= 72;
	for (int i = 0; i < time_us; i++) ;
//	uint32_t fac_us = SysTick->LOAD / 1000;
//	time_us = SysTick->VAL + time_us * fac_us;
//	if (time_us > SysTick->LOAD) time_us -= SysTick->LOAD;
//	while (true)
//	{
//		int dt = SysTick->VAL - time_us;
//		if (dt > (SysTick->LOAD / 2)) dt -= SysTick->LOAD;
//		else if (dt < (SysTick->LOAD / -2)) dt += SysTick->LOAD;
//		if (dt >= 0) break;
//	}
}
//计算两个正角度加法函数
int AngleAdd(int angle1, int angle2)
{
	int add = angle1 + angle2;
	if (add >= CODER_MAX_VAR) add -= CODER_MAX_VAR;
	return add;
}
//计算两个正角度减法函数
int AngleSub(int angle1, int angle2)
{
	int sub = angle1 - angle2;
	if (sub > (CODER_MAX_VAR / 2)) sub -= CODER_MAX_VAR;
	else if (sub < (-CODER_MAX_VAR / 2)) sub += CODER_MAX_VAR;
	return sub;
}
/*
********************************************************************************
*描    述 : 分别采集多路编码器数据。结果在Angle[]中.
*返    回 : 0-失败, 1-成功
********************************************************************************/
void ScanCoder(int index/*编码器索引号*/)
{
	switch (index)
	{
	case 0:
		HAL_GPIO_WritePin(nCS_AI1_GPIO_Port, nCS_AI1_Pin, GPIO_PIN_RESET);
		break;
	case 1:
		HAL_GPIO_WritePin(nCS_AI2_GPIO_Port, nCS_AI2_Pin, GPIO_PIN_RESET);
		break;
	case 2:
		HAL_GPIO_WritePin(nCS_AI3_GPIO_Port, nCS_AI3_Pin, GPIO_PIN_RESET);
		break;
	case 3:
		HAL_GPIO_WritePin(nCS_AI4_GPIO_Port, nCS_AI4_Pin, GPIO_PIN_RESET);
		break;
	default:
		return;
	}
	delay_us(1);//osDelay(2);
	CoderData_t data;
	int ret = MT6701_GetRawData(&data);
	delay_us(1);
	switch (index)
	{
	case 0:
		HAL_GPIO_WritePin(nCS_AI1_GPIO_Port, nCS_AI1_Pin, GPIO_PIN_SET);
		break;
	case 1:
		HAL_GPIO_WritePin(nCS_AI2_GPIO_Port, nCS_AI2_Pin, GPIO_PIN_SET);
		break;
	case 2:
		HAL_GPIO_WritePin(nCS_AI3_GPIO_Port, nCS_AI3_Pin, GPIO_PIN_SET);
		break;
	case 3:
		HAL_GPIO_WritePin(nCS_AI4_GPIO_Port, nCS_AI4_Pin, GPIO_PIN_SET);
		break;
	default:
		HAL_GPIO_WritePin(nCS_AI1_GPIO_Port, nCS_AI1_Pin, GPIO_PIN_SET);
		HAL_GPIO_WritePin(nCS_AI2_GPIO_Port, nCS_AI2_Pin, GPIO_PIN_SET);
		HAL_GPIO_WritePin(nCS_AI3_GPIO_Port, nCS_AI3_Pin, GPIO_PIN_SET);
		HAL_GPIO_WritePin(nCS_AI4_GPIO_Port, nCS_AI4_Pin, GPIO_PIN_SET);
		break;
	}
	
	if (ret == 0)
	Angle[index] = data.angle; // * (360.0f / 16384.0f);
}
//规格化模拟量
uint16_t NormalizedAnalog(int index/*模拟量索引号*/)
{
	if (EEPROM_MSG.analog_tap_pos[index] == 0xFFFF ||
		EEPROM_MSG.analog_tap_pos[index + 1] == 0xFFFF)
	{
		return Angle[index] | 0x8000; //不进行规格化,指示元码返回
	}
	int del = AngleSub(Angle[index], EEPROM_MSG.analog_tap_pos[index]);
	int range = AngleSub(EEPROM_MSG.analog_tap_pos[index + 1], EEPROM_MSG.analog_tap_pos[index]);
	del = del * 1000 / range;
	if (del < 0) //有新的最小值出现
	{
		EEPROM_MSG.analog_tap_pos[index] = Angle[index];
		del = 0;
	}
	else if (del > 1000) //有新的最大值出现
	{
		EEPROM_MSG.analog_tap_pos[index+1] = Angle[index];
		del = 1000;
	}
	return del;
}
//将模拟量转换为开关档位值
uint16_t CountTapPosition(int index/*模拟量索引号*/)
{
	uint8_t tap = 0;
	for (; tap < 10; tap++)	//支持最多10档
	{
		if (EEPROM_MSG.analog_tap_pos[index * 10 + tap] == 0xFFFF) {tap = 10; break;} //结束
		int del = AngleSub(Angle[index], EEPROM_MSG.analog_tap_pos[index * 10 + tap]);
		if (-46 < del && del < 46) break; //误差约为1°
	}
	return tap;
}
#pragma endregion


#pragma region //===方法===
void UpdateOut(void);
/*
********************************************************************************
*描    述 :	扫描传感器任务,包括按键,编码器。
*返    回 :	无
********************************************************************************/
void StartDefaultTask(void const * argument)
{
	//====局部变量定义
	uint32_t wake_time = 0;

	//====初始化
	//----采集模拟量
	HAL_GPIO_WritePin(nCS_AI1_GPIO_Port, nCS_AI1_Pin, GPIO_PIN_SET);
	HAL_GPIO_WritePin(nCS_AI2_GPIO_Port, nCS_AI2_Pin, GPIO_PIN_SET);
	HAL_GPIO_WritePin(nCS_AI3_GPIO_Port, nCS_AI3_Pin, GPIO_PIN_SET);
	HAL_GPIO_WritePin(nCS_AI4_GPIO_Port, nCS_AI4_Pin, GPIO_PIN_SET);
	//基于角度位置编码器SSI模式: 10.5<15.625MHz
	MT6701_Init(&hspi3, SPI3_CpltSemHandle);
	//----初始化输出开关量接口
	HAL_TIM_PWM_Start(&htim4, TIM_CHANNEL_4); //F0
	HAL_TIM_PWM_Start(&htim4, TIM_CHANNEL_3); //F1
	HAL_TIM_PWM_Start(&htim4, TIM_CHANNEL_2); //F2
	HAL_TIM_PWM_Start(&htim4, TIM_CHANNEL_1); //F3
	HAL_TIM_PWM_Start(&htim8, TIM_CHANNEL_4); //F4
	HAL_TIM_PWM_Start(&htim8, TIM_CHANNEL_3); //O0
	HAL_TIM_PWM_Start(&htim8, TIM_CHANNEL_2); //O1
	HAL_TIM_PWM_Start(&htim8, TIM_CHANNEL_1); //O2
	HAL_TIM_PWM_Start(&htim5, TIM_CHANNEL_1); //out1=O3
//	HAL_TIM_PWM_Start(&htim3, TIM_CHANNEL_2); //out2
//	HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_1); //out3
	HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_4); //out4=O4
	HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_1); //out5=O5
	HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_4); //out6=O6
	HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_3); //out7=O7
	HAL_TIM_PWM_Start(&htim3, TIM_CHANNEL_4); //out8=O8
	HAL_TIM_PWM_Start(&htim3, TIM_CHANNEL_3); //out9=O9
	
	osThreadSuspend(defaultTaskHandle);
	UpdateOut();
	wake_time = osKernelSysTick();
	//====循环体
	for (;;)
	{
		osDelayUntil(&wake_time, SAMPLE_T);
		//扫描模拟量
		for (size_t i = 0; i < 4; i++)
		{
			uint8_t j = EEPROM_MSG.analog_pos[i];
			if (j >= 0x80) continue; //不采集
			ScanCoder(i);
			Analogs[i] = (j < 63) ? NormalizedAnalog(i) : CountTapPosition(i);
			if (i < 3) osDelay(2);
		}
		//扫描开关量
		uint8_t keys[4];
		memset(keys, 0, 4);
		for (size_t i = 0; i < 10; i++)
		{
			uint8_t j = EEPROM_MSG.key_pos[i];
			if (j >= 0x80) continue; //不采集
			if (HAL_GPIO_ReadPin(In_Ports[i], In_Pins[i]) == GPIO_PIN_RESET)
				keys[j & 0x0F] |= (1 << (j >> 4)); //位置1
			else
				keys[j & 0x0F] &= ~(1 << (j >> 4)); //位清0
		}
		memcpy(Keys, keys, 4);
	}
}

#pragma endregion
